#
#    Copyright (C) 2016 Francisco Javier Parra <franparpe@openmailbox.org>
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
#

import math
import numpy
import matplotlib.pyplot as pyplot
# from UnitChanger import Radiants2Degrees  # Problem here
from qtcam.core.parameters import SectionParameters


class BezierSection(SectionParameters):
    def __init__(self, LowestPosition=0.0,
                 HighestPosition=50.0, StartAngle=0.0,
                 EndAngle=100.0, Precision=0.1, AngularVelocity=1,
                 Continuity=2, vector="double detention"):
        super(BezierSection, self).__init__(LowestPosition,
                                            HighestPosition,
                                            StartAngle,
                                            EndAngle,
                                            Precision,
                                            AngularVelocity)
        self.continuity = Continuity
        self.vector = vector
        self.showBezierOrdinates = False
        self.WhatAmI = "Bezier"
        self.BezierOrdinate = []
        self.BezPosOrdinate = []
        self.BezVelOrdinate = []
        self.BezAccOrdinate = []
        self.BezJerkOrdinates = []
        self.BezierAbcissaePos = []
        self.BezierAbcissaeVel = []
        self.BezierAbcissaeAcc = []
        self.BezierAbcissaeJerk = []
        
    def set_vector(self, vector="switch"):

        if vector == "switch":
            if self.vector == "simple detention":
                self.vector = "double detention"
            else:
                self.vector = "simple detention"
        elif isinstance(vector, list):
            self.vector = vector
        else:
            print("Error")

    def set_graph(self):
        """Modify the graph of the Bezier curve
        
        """
        self.showBezierOrdinates = not self.showBezierOrdinates

    def set_continuity(self, continuity):
        """
        Keyword Arguments:
        continuity -- This number set the continuity
        """
        self.continuity = continuity

    def BernsteinBase(self, currentDegree, angle):
        coeficient = (math.factorial(self.degree) /
                      (math.factorial(currentDegree) *
                       math.factorial(self.degree - currentDegree)))

        u = ((angle - self.StartAngle) /
             (self.EndAngle - self.StartAngle))

        return (coeficient * numpy.power(u, currentDegree) *
                numpy.power(1 - u, self.degree - currentDegree))

    def BernsteinBaseUnit(self, currentDegree, u):
        """Returns the same value that the method «BernsteinBase»
        but you have to introduce the unit value instead of the angle
        """
        coeficient = (math.factorial(self.degree) /
                      (math.factorial(currentDegree) *
                       math.factorial(self.degree - currentDegree)))

        return (coeficient * numpy.power(u, currentDegree) *
                numpy.power(1 - u, self.degree - currentDegree))

    def CreateBezierOrdinateSimpleDetention(self):
        """Creates the ordinate Bezier values corresponding
        to the displacement with simple detention
        
        """
        maximumBezierOrdinateIndex = ((self.numberOfControlPoints) // 2)
        maximumBezierOrdinateValue = (self.HighestPosition /
                                      self.BernsteinBaseUnit(maximumBezierOrdinateIndex, 0.5))
        ordinateVector = []

        # We start from zero, that is why we don't use 
        while len(ordinateVector) < (self.numberOfControlPoints // 2):
            ordinateVector.append(self.LowestPosition)

        ordinateVector.append(maximumBezierOrdinateValue)

        while len(ordinateVector) < self.numberOfControlPoints:
            ordinateVector.append(self.LowestPosition)

        return ordinateVector

    def CreateBezierOrdinateDoubleDetention(self):
        """Creates a simple array with the ordinate
        Bezier values that corresponds to a simple rise or fall motion

        """

        ordinateVector = []
        #if self.HighestPosition >= self.LowestPosition: # Rise
        while len(ordinateVector) < (self.numberOfControlPoints // 2):
            ordinateVector.append(self.LowestPosition)
        while len(ordinateVector) < self.numberOfControlPoints:
            ordinateVector.append(self.HighestPosition)
        # else: # Fall
        #     while len(ordinateVector) < (self.numberOfControlPoints // 2):
        #         ordinateVector.append(self.HighestPosition)
        #     while len(ordinateVector) < self.numberOfControlPoints:
        #         ordinateVector.append(self.LowestPosition)

        return ordinateVector
    
    def ArrayPosition(self):
        """Returns an array with all vallues of the position
        
        vector --: 0 means simple detention
        """
        angles = self.abcissaeValues
        result = 0
        if isinstance(self.vector, list):
            self.numberOfControlPoints = len(self.vector)
            self.BezierOrdinate = self.vector
            self.degree = len(self.vector) - 1
        elif not isinstance(self.vector, list):
            if self.vector == "simple detention":
                self.numberOfControlPoints = 2 * (self.continuity + 1) + 1
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateSimpleDetention()
            else:
                self.numberOfControlPoints = 2 * (self.continuity + 1)
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateDoubleDetention()
        else:
            print("error")  # An exception goes here

            self.BezPosOrdinate = self.BezierOrdinate

        currentDegree = 0
        while currentDegree <= self.degree:
            result += (self.BezierOrdinate[currentDegree] *
                       self.BernsteinBase(currentDegree, angles))
            currentDegree += 1

        return result

    def ArrayVelocity(self,):
        """Returns an array with all values of the velocity
        
        """
        angles = self.abcissaeValues
        result = 0
        if isinstance(self.vector, list):
            self.BezierOrdinate = self.vector
            self.degree = len(self.vector) - 1
        elif not isinstance(self.vector, list):
            if self.vector == "simple detention":
                self.numberOfControlPoints = 2 * (self.continuity + 1) + 1
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateSimpleDetention()
            else:
                self.numberOfControlPoints = 2 * (self.continuity + 1)
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateDoubleDetention()
        else:
            print("error")  # An exception goes here

        self.DerivateBezierOrdinates()

        self.BezVelOrdinate = self.BezierOrdinate

        currentDegree = 0
        while currentDegree <= self.degree:
            result += (self.BezierOrdinate[currentDegree] *
                       self.BernsteinBase(currentDegree, angles))
            currentDegree += 1

        return result

    def ArrayAcceleration(self,):
        """Returns an array with all values of the velocity
        
        """
        angles = self.abcissaeValues
        result = 0
        if isinstance(self.vector, list):
            self.BezierOrdinate = self.vector
            self.degree = len(self.vector) - 1
        elif not isinstance(self.vector, list):
            if self.vector == "simple detention":
                self.numberOfControlPoints = 2 * (self.continuity + 1) + 1
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateSimpleDetention()
            else:
                self.numberOfControlPoints = 2 * (self.continuity + 1)
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateDoubleDetention()
        else:
            print("error")  # An exception goes here

        self.DerivateBezierOrdinates()
        self.DerivateBezierOrdinates()

        self.BezAccOrdinate = self.BezierOrdinate

        currentDegree = 0
        while currentDegree <= self.degree:
            result += (self.BezierOrdinate[currentDegree] *
                       self.BernsteinBase(currentDegree, angles))
            currentDegree += 1

        return result
    
    def ArrayOverAcceleration(self,):
        """Returns an array with all values of the velocity
        
        """
        angles = self.abcissaeValues
        result = 0
        if isinstance(self.vector, list):
            self.BezierOrdinate = self.vector
            self.degree = len(self.vector) - 1
        elif not isinstance(self.vector, list):
            if self.vector == "simple detention":
                self.numberOfControlPoints = 2 * (self.continuity + 1) + 1
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateSimpleDetention()
            else:
                self.numberOfControlPoints = 2 * (self.continuity + 1)
                self.degree = self.numberOfControlPoints - 1
                self.BezierOrdinate = self.CreateBezierOrdinateDoubleDetention()
        else:
            print("error")  # An exception goes here

        self.DerivateBezierOrdinates()
        self.DerivateBezierOrdinates()
        self.DerivateBezierOrdinates()

        self.BezJerkOrdinates = self.BezierOrdinate
        
        currentDegree = 0
        while currentDegree <= self.degree:
            result += (self.BezierOrdinate[currentDegree] *
                       self.BernsteinBase(currentDegree, angles))
            currentDegree += 1

        return result


    def DerivateBezierOrdinates(self):
        """Modify Bezier, and numberOfControlPoints
        to create the derivative expression
        
        """
        self.numberOfControlPoints -= 1
        print(self.numberOfControlPoints)
        self.degree -= 1
        print(self.degree)
        currentControlPoint = 0
        while currentControlPoint < self.numberOfControlPoints:
            # Modified math.dgrees
            self.BezierOrdinate[currentControlPoint] = ((self.numberOfControlPoints * self.AngularVelocity *
                                                        (self.BezierOrdinate[currentControlPoint + 1] -
                                                         self.BezierOrdinate[currentControlPoint])) /
                                                        math.radians(self.EndAngle - self.StartAngle))
            currentControlPoint += 1
        del self.BezierOrdinate[-1]
        
    def CreateYaxisPosition(self,):  # eliminar para crear algo mejor
        """This method initializes two arrays atributes, the first is the
        angle and the second is the Position"""
        self.ordinatePosition = self.ArrayPosition()
        currentDegree = 0
        ordinates = []
        self.BezierAbcissaePos = []
        for a in self.BezierOrdinate:
            ordinates.append(a)
            self.BezierAbcissaePos.append(((currentDegree / self.degree) *
                                          (self.EndAngle - self.StartAngle)) +
                                         self.StartAngle)
        print(self.BezierOrdinate)

    def CreateYaxisVelocity(self,):
        """This method creates the ordinate values for the Velocity"""
        self.ordinateVelocity = self.ArrayVelocity()
        currentDegree = 0
        ordinates = []
        self.BezierAbcissaeVel = []
        for a in self.BezierOrdinate:
            ordinates.append(a)
            self.BezierAbcissaeVel.append(((currentDegree / self.degree) *
                                          (self.EndAngle - self.StartAngle)) +
                                         self.StartAngle)
        print(self.BezierOrdinate)

    def CreateYaxisAcceleration(self,):
        """This method creates the ordinate values for the Acceleration"""
        self.ordinateAcceleration = self.ArrayAcceleration()
        currentDegree = 0
        ordinates = []
        self.BezierAbcissaeAcc = []
        for a in self.BezierOrdinate:
            ordinates.append(a)
            self.BezierAbcissaeAcc.append(((currentDegree / self.degree) *
                                          (self.EndAngle - self.StartAngle)) +
                                         self.StartAngle)
        print(self.BezierOrdinate)

        
    def CreateYaxisOverAcceleration(self,):
        """This method creates the ordinate values for the
        overacceleration"""
        self.ordinateOverAcceleration = self.ArrayOverAcceleration()
        currentDegree = 0
        ordinates = []
        self.BezierAbcissaeJerk = []
        for a in self.BezierOrdinate:
            ordinates.append(a)
            self.BezierAbcissaeJerk.append(((currentDegree / self.degree) *
                                          (self.EndAngle - self.StartAngle)) +
                                         self.StartAngle)
        print(self.BezierOrdinate)
